#pragma once
#include "ros/ros.h"
#include "auto_msgs/localizition.h"
#include "auto_msgs/chassis.h"
#include "auto_msgs/control_cmd.h"
#include "auto_msgs/planning.h"
#include "auto_msgs/planning_point.h"
#include "common_tool/clampPid.h"
#include "common_tool/watchDog.h"
#include <osqp_control/debug_lateral.h>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <memory>
#include "kf_factory.hpp"
#include "vehicle_lmpc_factory.hpp"
#include "node_base.h"

using namespace KalmanFilter;
namespace auto_ros
{
namespace control
{
class NodeLateralSpeed : public NodeBase
{
private:
	std::shared_ptr<VehicleLMpcBase> mpc_ptr_;
	std::shared_ptr<KfVehicleBase> kf_ptr_;
	Eigen::VectorXd x0_;
	Eigen::VectorXd u_fb_;
	double kappa_ = 0;
	double speed_ = 0;
	double speed_cmd_ = 0;
	double last_steer_angle_ = 0;
	bool reset_kf_ = true;

	//x0
	bool LateralCtr();
	bool LongtitudeCtr();

public:
	clampPid speed_controller_;
	NodeLateralSpeed() = delete;
	NodeLateralSpeed(double loop_rate, std::string node_yaml_file_name);
	virtual void LoopProc();
	virtual void HardBrake();
	virtual bool UpdateInput();
	virtual void UpdateDebugMsg();
};
} // namespace control
} // namespace auto_ros